#include <iostream>
#include <opencv2/opencv.hpp>
#include <queue>
#include <vector>
#include <algorithm>
/****************************************
 **********移动机器人规划控制入门与实践*******
 **************Dijkstra示例代码***********
 * 包含：
 * 图片转栅格地图；
 * dijkstra搜索算法；
 * 基于opencv的拓展过程显示。
 ****************************************
*/

//定义二维点
struct Point {
    int x;
    int y;
};

//辅助函数——标记障碍物
void markObstacles(cv::Mat &image, const std::vector<std::vector<int>> &grid) {
    for (int y = 0; y < grid.size(); y++) {
        for (int x = 0; x < grid[0].size(); x++) {
            if (grid[y][x] == 1) {
                // 使用黑色标记障碍物
                cv::Rect roi(x * 10, y * 10, 10, 10);
                image(roi) = cv::Vec3b(0, 0, 0); // Black color
            }
        }
    }
}
//辅助函数——标记起点
void markStart(cv::Mat &image, Point point) {
                // 使用蓝色标记起点
                cv::Rect roi(point.x * 10, point.y * 10, 10, 10);
                image(roi) = cv::Vec3b(255, 0, 0); // Blue color
}
//辅助函数——标记终点
void markGoal(cv::Mat &image, Point point) {
                // 使用深红标记终点
                cv::Rect roi(point.x * 10, point.y * 10, 10, 10);
                image(roi) = cv::Vec3b(0, 0, 125); // Red color       //Blue   Green   Red
}

const float INF = std::numeric_limits<float>::max(); // 无穷大

//定义优先级队列的比较函数
struct CompareDistance {
    bool operator()(const std::pair<float, Point>& a, const std::pair<float, Point>& b) {
        return a.first > b.first;
    }
};

//判断点是否在地图内
bool isValidPoint(const Point& p, int rows, int cols) {
    return (p.x >= 0 && p.x < cols && p.y >= 0 && p.y < rows);
}

/**************************************************************/
/*************************dijkstra搜索算法**********************/
/**************************************************************/
std::vector<Point> dijkstra(std::vector<std::vector<int>> &grid, Point start, Point end, cv::Mat &image) {
    int rows = grid.size();
    int cols = grid[0].size();
    std::vector<std::vector<float>> distance(rows, std::vector<float>(cols, INF));
    std::vector<std::vector<Point>> parent(rows, std::vector<Point>(cols, { -1, -1 }));
    std::priority_queue<std::pair<float, Point>, std::vector<std::pair<float, Point>>, CompareDistance> pq;

    distance[start.y][start.x] = 0;
    pq.push({0, start});

    while (!pq.empty()) {
        Point current = pq.top().second;
        float current_dis = pq.top().first;
        pq.pop();

        if(current_dis > distance[current.y][current.x]){   //跳过距离不是最优的节点
            std::cout<<"skip an old node!"<<std::endl;
            continue;
        }
            
        if (current.x == end.x && current.y == end.y) {
            // 找到目标点，可以退出
            std::vector<Point> path;
            Point current = end;
            while (current.x != -1 && current.y != -1) {
                path.push_back(current);
                current = parent[current.y][current.x];
            }
    
            // 反转路径，使其从起点到终点
            std::reverse(path.begin(), path.end());
            return path;
        }
        
        //八连通地图
        int dx[] = {1, -1, 0, 0, 1, -1, 1, -1};
        int dy[] = {0, 0, 1, -1, 1, 1, -1, -1};

        for (int i = 0; i < 4; ++i) {
            int nx = current.x + dx[i];
            int ny = current.y + dy[i];

            if (isValidPoint({nx, ny}, rows, cols) && grid[ny][nx] == 0) {
                float new_distance = distance[current.y][current.x] + std::sqrt(dx[i]*dx[i] + dy[i]*dy[i]); // 1 表示相邻节点的代价
                if (new_distance < distance[ny][nx]) {  //这里没有执行删除原节点的操作---priority_queue没有提供指定point删除的操作，复杂度较高
                    distance[ny][nx] = new_distance;
                    parent[ny][nx] = current;
                    pq.push({new_distance, {nx, ny}});

                    // 显示该节点
                    if(!(nx==end.x && ny==end.y))
                    {
                        cv::Rect roi(nx * 10, ny * 10, 10, 10);
                        image(roi) = cv::Vec3b(0, 255, 0); // Green color
                        cv::imshow("Dijkstra Expansion", image);
                        cv::waitKey(1);
                    }
                }
            }
        }
    }

    // No path found
    return {};
}

//统计路径长度
double calculatePathLength(const std::vector<Point>& path) {
    double length = 0.0;
    for (size_t i = 1; i < path.size(); ++i) {
        // 计算相邻两点之间的欧氏距离并累加
        double dx = path[i].x - path[i - 1].x;
        double dy = path[i].y - path[i - 1].y;
        length += std::sqrt(dx * dx + dy * dy);
    }
    return length;
}

/*无障碍物*/
// std::string pic="0.png";   //设置地图
// Point start = {40, 50};    // 设置起点
// Point end = {70, 50};      //设置终点

/*有障碍物*/
std::string pic="1.png";   //设置地图
Point start = {20, 50};    // 设置起点
Point end = {88, 50};      //设置终点

int main() {
    // 读取图片
    cv::Mat image = cv::imread(pic);

    if (image.empty()) {
        std::cerr << "无法读取图像文件" << std::endl;
        return -1;
    }

    // 缩放图片到所需的尺寸（栅格地图的大小）
    cv::Size new_size(100, 100); // 根据需要调整大小
    cv::resize(image, image, new_size, 0, 0, cv::INTER_LINEAR);

    // 创建栅格地图，0表示可通行，1表示障碍
    std::vector<std::vector<int>> grid_map;

    for (int y = 0; y < new_size.height; y++) {
        std::vector<int> row;
        for (int x = 0; x < new_size.width; x++) {
            cv::Vec3b pixel = image.at<cv::Vec3b>(y, x);
            if (pixel == cv::Vec3b(0, 0, 0)) { // 假设黑色为障碍
                row.push_back(1);
            } else {
                row.push_back(0);
            }
        }
        grid_map.push_back(row);
    }

    // 打印栅格地图
    for (const auto& row : grid_map) {
        for (int value : row) {
            std::cout << value << " ";
        }
        std::cout << std::endl;
    }
    
    //坐标系定义：
    // Ori*---------->x
    //    |
    //    |
    //    |
    //    |
    //    v
    //    y

    // // 设置起、终点
    // Point start = {20, 50};
    // Point end = {88, 50};

    //显示地图
    cv::Mat output_image(grid_map.size() * 10, grid_map[0].size() * 10, CV_8UC3, cv::Scalar(255, 255, 255));
    markStart(output_image,start);
    markGoal(output_image,end);
    markObstacles(output_image, grid_map); // 标记障碍物

    //搜索路径
    std::vector<Point> path = dijkstra(grid_map, start, end, output_image);

    cv::waitKey(0); // Wait for user input

    //打印、显示路径
    if (path.empty()) {
        std::cout << "未找到路径" << std::endl;
    } else {
        std::cout << "找到路径，路径长度: " << calculatePathLength(path)<< std::endl;
        //显示路径，但是不显示起点和终点
         for (unsigned int i=1;i<path.size()-1;i++) {
            std::cout << "(" << path[i].x << ", " << path[i].y << ") -> ";
            // Mark the path in red
            cv::Rect roi(path[i].x * 10, path[i].y * 10, 10, 10);
            output_image(roi) = cv::Vec3b(0, 0, 255); // Red color
        }
        std::cout << std::endl;
    }

    cv::imshow("Dijkstra Expansion with Path", output_image);
    cv::waitKey(0); // Wait for user input

    return 0;
}

